#include <cassert>
#include <boost/algorithm/string.hpp>
#include <map>

#include "../../../XMLTools.h"

#include "ModelPoseSensor.h"

using namespace MMM;

ModelPoseSensorPtr ModelPoseSensor::loadSensorXML(RapidXMLReaderNodePtr node) {
    ModelPoseSensorPtr sensor(new ModelPoseSensor());
    sensor->loadSensor(node);
    return sensor;
}

ModelPoseSensor::ModelPoseSensor(const std::string &description) : InterpolatableSensor(description)
{
}

bool ModelPoseSensor::checkModel(ModelPtr model) {
    if (!model) {
        MMM_ERROR << "Sensor '" << uniqueName << "' needs a model."  << std::endl;
        return false;
    }
    return true;
}

boost::shared_ptr<BasicSensor<ModelPoseSensorMeasurement> > ModelPoseSensor::cloneConfiguration() {
    ModelPoseSensorPtr m(new ModelPoseSensor(description));
    return m;
}

void ModelPoseSensor::loadConfigurationXML(RapidXMLReaderNodePtr node) {
    assert(node);
    assert(node->name() == "Configuration");
}

void ModelPoseSensor::loadMeasurementXML(RapidXMLReaderNodePtr node, float timestep, SensorMeasurementType type)
{
    assert(node);
    assert(node->name() == XML::NODE_MEASUREMENT);

    RapidXMLReaderNodePtr posNode = node->first_node("RootPosition");
    RapidXMLReaderNodePtr rotNode = node->first_node("RootRotation");
    ModelPoseSensorMeasurementPtr m(new ModelPoseSensorMeasurement(timestep, XML::readFloatArray(posNode->value(), 3), XML::readFloatArray(rotNode->value(), 3), type));
    addSensorMeasurement(m);
}

void ModelPoseSensor::appendConfigurationXML(RapidXMLWriterNodePtr node)
{
    assert(node);

    node->append_node("Configuration");
}

bool ModelPoseSensor::hasSensorConfigurationContent() const
{
    return true; // TODO
}

bool ModelPoseSensor::equalsConfiguration(SensorPtr other) {
    ModelPoseSensorPtr ptr = boost::dynamic_pointer_cast<ModelPoseSensor>(other);
    if (ptr) {
        return true;
    }
    else return false;
}

std::string ModelPoseSensor::getType() {
    return TYPE;
}

std::string ModelPoseSensor::getVersion() {
    return VERSION;
}

int ModelPoseSensor::getPriority() {
    return 100;
}
